Inverse Kinematics recap

Now that you have a good understanding of the robot and its Forward Kinematics, let us do a quick recap of Inverse Kinematics and figure out how to use it in this project.

Since the last three joints in our robot are revolute and their joint axes intersect at a single point, we have a case of spherical wrist with joint_5 being the common intersection point and hence the wrist center.

This allows us to kinematically decouple the IK problem into Inverse Position and Inverse Orientation problems as discussed in the Inverse Kinematics theory lesson.

First let us solve for the Inverse Position problem. Since we have the case of a spherical wrist involving joints 4,5,6, the position of the wrist center is governed by the first three joints. We can obtain the position of the wrist center by using the complete transformation matrix we derived in the last section based on the end-effector pose.

For the sake of simplification, let us symbolically define our homogeneous transform as following:

where l, m and n are orthonormal vectors representing the end-effector orientation along X, Y, Z axes of the local coordinate frame.

Since n is the vector along the z-axis of the gripper_link, we can say the following:

Where,

Px, Py, Pz = end-effector positions

Wx, Wy, Wz = wrist positions

d6 = from DH table

l = end-effector length

Now, in order to calculate nx, ny, and nz, let's continue from the previous section where you calculated the rotation matrix to correct the difference between the URDF and the DH reference frames for the end-effector.

After you obtain this correctional rotation matrix, you need to next calculate your end-effector pose with respect to the base_link. In the Compositions of Rotations section, we covered different conventions regarding Euler angles, and how to choose the correct convention.

One such convention is the x-y-z extrinsic rotations. The resulting rotational matrix using this convention to transform from one fixed frame to another, would be:

Rrpy = Rot(Z, yaw) * Rot(Y, pitch) * Rot(X, roll) * R_corr

Where R_corr is the correctional rotation matrix.

The roll, pitch, and yaw values for the gripper are obtained from the simulation in ROS. But since these values are returned in quaternions, you can use the transformations.py module from the TF package. The euler_from_quaternions() method will output the roll, pitch, and yaw values.

You can then extract nx, ny, and nz values from this Rrpy matrix to obtain the wrist center position.

Now that you have the wrist center position, follow the simple math presented in this section to derive the equations for first three joints.

Calculating theta 1 will be relatively straightforward once you have the wrist center position from above. Theta 2 and theta 3 can be relatively tricky to visualize. The following diagram depicts the angles for you.

The labels 2, 3 and WC are Joint 2, Joint 3, and the Wrist Center, respectively. You can obtain, or rather visualize, the triangle between the three if you project the joints onto the z-y plane corresponding to the world reference frame. From your DH parameters, you can calculate the distance between each joint above. Using trigonometry, specifically the Cosine Laws, you can calculate theta 2 and theta 3.

For the Inverse Orientation problem, we need to find values of the final three joint variables.

Using the individual DH transforms we can obtain the resultant transform and hence resultant rotation by:

R0_6 = R0_1*R1_2*R2_3*R3_4*R4_5*R5_6

Since the overall RPY (Roll Pitch Yaw) rotation between base_link and gripper_link must be equal to the product of individual rotations between respective links, following holds true:

R0_6 = Rrpy

where,

Rrpy = Homogeneous RPY rotation between base_link and gripper_link as calculated above.

We can substitute the values we calculated for joints 1 to 3 in their respective individual rotation matrices and pre-multiply both sides of the above equation by inv(R0_3) which leads to:

R3_6 = inv(R0_3) * Rrpy

The resultant matrix on the RHS (Right Hand Side of the equation) does not have any variables after substituting the joint angle values, and hence comparing LHS (Left Hand Side of the equation) with RHS will result in equations for joint 4, 5, and 6.

Note: While calculating the inverse above, using Sympy's inv() method, please make sure to pass "LU" as an argument to ensure that it's calculated using the LU decomposition. You can refer to the documentation on how to use it here.

Finally, we have equations describing all six joint variables, next we will turn our kinematic analysis into a ROS python node which will perform the Inverse Kinematics for the pick and place operation.